{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "4278f0a7-30cf-422f-b4aa-2bdd90db7299",
   "metadata": {},
   "source": [
    "# 单电机实验 demo"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "72147c7a-2d49-49fb-87b7-f40b02bf1b74",
   "metadata": {},
   "source": [
    "## 初始化电机"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "ae0d945a-b02e-4062-a10d-f6655834d8b2",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "from robodyno.components.can_bus.mi_motor import MiMotor\n",
    "from robodyno.interfaces import CanBus\n",
    "can = CanBus()\n",
    "\n",
    "motor = MiMotor(can, 0x7F)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "abaf865c-2c3a-4c5a-ba4c-08de67f14ba3",
   "metadata": {},
   "source": [
    "## 使能"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "600e1068-b22a-4d4f-ac7e-1893d5ff46c0",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "motor.enable()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b16336f8",
   "metadata": {},
   "source": [
    "## 控制"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "75519cb1",
   "metadata": {},
   "outputs": [],
   "source": [
    "import ipywidgets as widgets\n",
    "from IPython.display import display\n",
    "import time\n",
    "import threading\n",
    "import numpy as np\n",
    "from collections import deque\n",
    "\n",
    "thread_running = True\n",
    "\n",
    "start_time = time.time()  # 获取开始时间\n",
    "\n",
    "# 数据存储（记录时间、位置、速度、力矩）\n",
    "MAX_DATA_POINTS = 1000\n",
    "time_data = deque(maxlen=MAX_DATA_POINTS)\n",
    "pos_data = deque(maxlen=MAX_DATA_POINTS)\n",
    "vel_data = deque(maxlen=MAX_DATA_POINTS)\n",
    "torque_data = deque(maxlen=MAX_DATA_POINTS)\n",
    "\n",
    "# 电机使能按钮\n",
    "is_enabled = (motor.get_state()[0].value != 0)\n",
    "enable_motor_button = widgets.ToggleButton(\n",
    "    value=is_enabled,\n",
    "    description='电机已使能' if is_enabled else '电机已失能',\n",
    "    button_style='success' if is_enabled else 'danger',\n",
    "    icon='check' if is_enabled else 'times',\n",
    "    tooltip='启用或禁用电机',\n",
    ")\n",
    "def on_toggle(change):\n",
    "    if change.new:  # 启用电机\n",
    "        motor.enable()\n",
    "        enable_motor_button.description = '电机已使能'\n",
    "        enable_motor_button.button_style = 'success'\n",
    "        enable_motor_button.icon = 'check'\n",
    "    else:  # 禁用电机\n",
    "        motor.disable()\n",
    "        enable_motor_button.description = '电机已失能'\n",
    "        enable_motor_button.button_style = 'danger'\n",
    "        enable_motor_button.icon = 'times'\n",
    "enable_motor_button.observe(on_toggle, names='value')\n",
    "\n",
    "# 初始化位置按钮\n",
    "init_pos_buttom = widgets.Button(description='初始化位置')\n",
    "def on_init_pos_button_clicked(b):\n",
    "    motor.init_pos()\n",
    "init_pos_buttom.on_click(on_init_pos_button_clicked)\n",
    "\n",
    "# 数据显示控件\n",
    "pos_label = widgets.Label(value=\"0.00 rad\", description=\"位置:\")\n",
    "vel_label = widgets.Label(value=\"0.00 rad/s\", description=\"速度:\")\n",
    "torque_label = widgets.Label(value=\"0.00 Nm\", description=\"力矩:\")\n",
    "voltage_label = widgets.Label(value=\"0.00 V\", description=\"电压:\")\n",
    "temp_label = widgets.Label(value=\"0.00 °C\", description=\"温度:\")\n",
    "\n",
    "# 数据显示区域布局\n",
    "data_display_area = widgets.VBox([\n",
    "    widgets.HTML(\"<b>实时数据</b>\"),  # 标题\n",
    "    widgets.HBox([widgets.Label(\"位置:\"), pos_label], layout=widgets.Layout(margin=\"5px\")),\n",
    "    widgets.HBox([widgets.Label(\"速度:\"), vel_label], layout=widgets.Layout(margin=\"5px\")),\n",
    "    widgets.HBox([widgets.Label(\"力矩:\"), torque_label], layout=widgets.Layout(margin=\"5px\")),\n",
    "    widgets.HBox([widgets.Label(\"电压:\"), voltage_label], layout=widgets.Layout(margin=\"5px\")),\n",
    "    widgets.HBox([widgets.Label(\"温度:\"), temp_label], layout=widgets.Layout(margin=\"5px\"))\n",
    "])\n",
    "\n",
    "# 控制模式切换按钮\n",
    "mode = motor.get_mode()[0].value\n",
    "toggle_buttons = widgets.ToggleButtons(\n",
    "    options=[('MIT', 0), ('位置', 1), ('速度', 2), ('力矩', 3)],\n",
    "    description='控制模式:',\n",
    "    value=mode\n",
    ")\n",
    "# 参数输入控件\n",
    "mit_controls = widgets.VBox([\n",
    "    widgets.FloatText(description='Torque:', value=0.0),\n",
    "    widgets.FloatText(description='Position:', value=0.0),\n",
    "    widgets.FloatText(description='Velocity:', value=0.0),\n",
    "    widgets.FloatText(description='Kp:', value=0.0),\n",
    "    widgets.FloatText(description='Kd:', value=0.0),\n",
    "    widgets.Button(description='设置MIT参数')\n",
    "])\n",
    "pos_controls = widgets.VBox([\n",
    "    widgets.FloatText(description='位置:', value=0.0),\n",
    "    widgets.FloatText(description='速度限制:', value=30.0),\n",
    "    widgets.Button(description='设置位置参数')\n",
    "])\n",
    "vel_controls = widgets.VBox([\n",
    "    widgets.FloatText(description='速度:', value=0.0),\n",
    "    widgets.FloatText(description='电流限制:', value=20.0),\n",
    "    widgets.Button(description='设置速度参数')\n",
    "])\n",
    "torque_controls = widgets.VBox([\n",
    "    widgets.FloatText(description='力矩:', value=0.0),\n",
    "    widgets.Button(description='设置力矩参数')\n",
    "])\n",
    "def update_controls(mode):\n",
    "    if mode == 0:  # MIT 模式\n",
    "        display_area.children = [mit_controls]\n",
    "    elif mode == 1:  # 位置模式\n",
    "        display_area.children = [pos_controls]\n",
    "    elif mode == 2:  # 速度模式\n",
    "        display_area.children = [vel_controls]\n",
    "    elif mode == 3:  # 力矩模式\n",
    "        display_area.children = [torque_controls]\n",
    "def on_value_change(change):\n",
    "    mode = change.new\n",
    "    update_controls(mode)\n",
    "toggle_buttons.observe(on_value_change, names='value')\n",
    "\n",
    "# 为每个模式的设置按钮添加功能\n",
    "def set_mit_params(_):\n",
    "    motor.mit_mode()\n",
    "    params = [control.value for control in mit_controls.children[:-1]]\n",
    "    motor.set_mit(*params)\n",
    "\n",
    "def set_pos_params(_):\n",
    "    motor.position_mode()\n",
    "    pos = pos_controls.children[0].value\n",
    "    vel_limit = pos_controls.children[1].value\n",
    "    motor.set_vel_limit(vel_limit)\n",
    "    motor.set_pos(pos)\n",
    "\n",
    "\n",
    "def set_vel_params(_):\n",
    "    motor.velocity_mode()\n",
    "    velocity = vel_controls.children[0].value\n",
    "    current_limit = vel_controls.children[1].value\n",
    "    motor.set_current_limit(current_limit)\n",
    "    motor.set_vel(velocity)\n",
    "\n",
    "\n",
    "def set_torque_params(_):\n",
    "    motor.torque_mode()\n",
    "    torque = torque_controls.children[0].value\n",
    "    motor.set_torque(torque)\n",
    "\n",
    "# 为按钮绑定事件\n",
    "mit_controls.children[-1].on_click(set_mit_params)\n",
    "pos_controls.children[-1].on_click(set_pos_params)\n",
    "vel_controls.children[-1].on_click(set_vel_params)\n",
    "torque_controls.children[-1].on_click(set_torque_params)\n",
    "\n",
    "# 控件显示区域\n",
    "display_area = widgets.VBox()\n",
    "update_controls(mode)\n",
    "\n",
    "\n",
    "# 显示所有控件\n",
    "\n",
    "display(enable_motor_button, init_pos_buttom, data_display_area, toggle_buttons, display_area)\n",
    "\n",
    "\n",
    "\n",
    "# 数据实时更新函数\n",
    "def update_data_display():\n",
    "    while thread_running:\n",
    "        try:\n",
    "            # 获取实时数据\n",
    "            _, _, _, p, v, t, _ = motor._mi_get_state()\n",
    "            pos = motor.get_pos(timeout=1)\n",
    "            vel = motor.get_vel(timeout=1)\n",
    "            torque = motor.get_torque(timeout=1)\n",
    "            torque = t\n",
    "            voltage = motor.get_voltage(timeout=1)\n",
    "            temp = motor.get_temperature(timeout=1)\n",
    "\n",
    "            # 获取当前时间\n",
    "            current_time = time.time() - start_time\n",
    "\n",
    "            if pos is None or vel is None or torque is None or voltage is None or temp is None:\n",
    "                raise Exception(\"获取数据失败\")\n",
    "            else:\n",
    "                # 更新显示控件的值，固定小数位数和宽度\n",
    "                pos_label.value = f\"{pos:+8.2f} rad\"        # 正数显示+，负数显示-\n",
    "                vel_label.value = f\"{vel:+8.2f} rad/s\"\n",
    "                torque_label.value = f\"{torque:+8.2f} Nm\"\n",
    "                voltage_label.value = f\"{voltage:+8.2f} V\"\n",
    "                temp_label.value = f\"{temp:+8.2f} °C\"\n",
    "                \n",
    "                # 更新数据记录\n",
    "                global time_data, pos_data, vel_data, torque_data\n",
    "                time_data.append(current_time)\n",
    "                pos_data.append(pos)\n",
    "                vel_data.append(vel)\n",
    "                torque_data.append(torque)\n",
    "\n",
    "        except Exception as e:\n",
    "            continue\n",
    "        \n",
    "        time.sleep(0.05)  # 更新频率\n",
    "\n",
    "# 启动数据更新线程\n",
    "update_thread = threading.Thread(target=update_data_display, daemon=True)\n",
    "update_thread.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8152270e",
   "metadata": {},
   "source": [
    "## 绘图"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "745895c9",
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "from matplotlib.animation import FuncAnimation\n",
    "\n",
    "# 创建 x 轴数据\n",
    "x = np.linspace(0, 2 * np.pi, 100)\n",
    "\n",
    "# 初始化图形\n",
    "fig, ax = plt.subplots()\n",
    "\n",
    "# 设置图形的初始内容\n",
    "line_sin, = ax.plot(time_data, pos_data, label=\"pos\", color='blue')\n",
    "line_cos, = ax.plot(time_data, vel_data, label=\"vel\", color='green')\n",
    "line_cos, = ax.plot(time_data, torque_data, label=\"torque\", color='red')\n",
    "\n",
    "# 设置标题和标签\n",
    "ax.set_title(\"Real-time Sine and Cosine Functions\")\n",
    "ax.set_xlabel(\"x\")\n",
    "ax.set_ylabel(\"y\")\n",
    "ax.legend()\n",
    "\n",
    "# 动画更新函数\n",
    "def update(frame):\n",
    "    # 更新 sin(x) 和 cos(x) 数据\n",
    "    line_sin.set_ydata(np.sin(x + frame / 10))  # 动态更新 sin(x)\n",
    "    line_cos.set_ydata(np.cos(x + frame / 10))  # 动态更新 cos(x)\n",
    "    return line_sin, line_cos\n",
    "\n",
    "# 创建动画\n",
    "ani = FuncAnimation(fig, update, frames=100, interval=50)\n",
    "\n",
    "# 显示图形\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0e801b52-ba80-46d4-b095-51f11c573ef6",
   "metadata": {},
   "source": [
    "## 停止"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "5e73ab89-3e0f-4820-8efe-e5730601fe4d",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "thread_running = False\n",
    "update_thread.join()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "60b9dc14-ae16-441c-add3-71efb49be2fd",
   "metadata": {},
   "source": [
    "## 电机失能"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "2eee1033-526b-4541-bf2f-472329ffe242",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "motor.disable()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c7d4172a-e37f-4283-8f21-852fb063db86",
   "metadata": {},
   "source": [
    "## 断开CAN总线"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "c3ea31d2-16cd-4a99-8c85-44d311d462cc",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "can.disconnect()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
